/*
*
==============================================================================
*  Name        : 
*  Part of     : Omen Game Engine
*  Description : 
*  Version     : 1.0
*
*  Copyright (c) 2013 Lauri Kortevaara.
* 
==============================================================================
*/

#define OMEN_SOURCE 1

#include <IOmenCollision.h>
#include <OmenMath.h>
#include <OmenVector3d.h>

using namespace Omen;

#include <iostream>
#include <sstream>

///
/// <summary>Sphere bounding volume</summary>
///
SphereBoundingVolume::SphereBoundingVolume( const Omen::Vector3d& position, Omen::Float radius ) :
    IBoundingVolume( BoundingVolume_Sphere ),
    m_sphere(Omen::Sphere(position,radius))
{
}

///
/// <summary>Returns true if the given bounding volume collides with this volume</summary>
///
bool SphereBoundingVolume::collides( IBoundingVolume* pVolume )
{
    bool collides = false;
    if( pVolume )
    {
        switch( pVolume->volumeType() )
        {
            case BoundingVolume_Sphere:
            {
                SphereBoundingVolume* pSphere = dynamic_cast<SphereBoundingVolume*>(pVolume);
                if( pSphere )
                {
                    return SphereToSphere( *this, *pSphere );
                }
                else return false;
                break;
            }
            case BoundingVolume_AABB:
            {
                AABBVolume* pBox = dynamic_cast<AABBVolume*>(pVolume);
                if( pBox )
                {
                    return SphereToAABB( *this, *pBox );
                }
                else return false;
                break;
                break;
            }
        }
    }
    return collides;
}

bool AABBVolume::collides( IBoundingVolume* pVolume )
{
    bool collides = false;
    if( pVolume )
    {
        switch( pVolume->volumeType() )
        {
            case BoundingVolume_Sphere:
            {
                SphereBoundingVolume* pSphere = dynamic_cast<SphereBoundingVolume*>(pVolume);
                if( pSphere )
                    return SphereToAABB(*pSphere, *this );
                else
                    return false;
                break;
            }
            case BoundingVolume_AABB:
            {
                AABBVolume* pBox = dynamic_cast<AABBVolume*>(pVolume);
                if( pBox )
                {
                    return AABBtoAABB( *this, *pBox );
                }
                else return false;
                break;
            }
        }
    }
    return collides;   
}

OMEN_API bool SphereToSphere( const Omen::SphereBoundingVolume& sphere1, const Omen::SphereBoundingVolume& sphere2 )
{
    //Calculate the squared distance between the centers of both spheres
    Omen::Vector3d p1 = sphere1.position();
    Omen::Vector3d p2 = sphere2.position();

    Omen::Vector3d vecDist( p2 );
    vecDist -= p1;

    float fDistSq( Omen::Vector3d::dotProduct( vecDist, vecDist ) );

    //Calculate the squared sum of both radii
    float fRadiiSumSquared( sphere1.radius() + sphere2.radius() );
    fRadiiSumSquared *= fRadiiSumSquared;

    //Check for collision
    //If the distance squared is less than or equal to the square sum
    //of the radii, then we have a collision
    if( fDistSq <= fRadiiSumSquared )
        return true;

    //If not, then return false
    return false;
}

static inline Omen::Float squared( Omen::Float x ) { return x*x; }

OMEN_API bool SphereToAABB(const SphereBoundingVolume& sphere, const AABBVolume& box )
{
    std::wstring str;
    std::wstringstream ss;
    
    float R = sphere.radius();
    float dist_squared = R * R;
    Omen::Vector3d S = sphere.position();
    Omen::Vector3d C1 = box.vmin();
    Omen::Vector3d C2 = box.vmax();
    /* assume C1 and C2 are element-wise sorted, if not, do that now */
    if (S.x < C1.x) dist_squared -= squared(S.x - C1.x);
    else if (S.x > C2.x) dist_squared -= squared(S.x - C2.x);
    if (S.y < C1.y) dist_squared -= squared(S.y - C1.y);
    else if (S.y > C2.y) dist_squared -= squared(S.y - C2.y);
    if (S.z < C1.z) dist_squared -= squared(S.z - C1.z);
    else if (S.z > C2.z) dist_squared -= squared(S.z - C2.z);

    //ss << L"Collision Detection: " << sphere.position().x << ", " << sphere.position().y << ", " << sphere.position().z << ", distSquared: " << dist_squared << std::endl;
    //std::wcout << ss.str() << std::endl;

    return dist_squared > 0;
}

OMEN_API bool AABBtoAABB(const AABBVolume& box1, const AABBVolume& box2)
{
    //Check if Box1's max is greater than Box2's min and Box1's min is less than Box2's max
    return( box1.vmax().x > box2.vmin().x && 
            box1.vmin().x < box2.vmax().x &&
            box1.vmax().y > box2.vmin().y &&
            box1.vmin().y < box2.vmax().y &&
            box1.vmax().z > box2.vmin().z &&
            box1.vmin().z < box2.vmax().z);
}

OMEN_API Omen::Float intersectRayPlane(Omen::Vector3d& rOrigin, Omen::Vector3d& rVector, Omen::Vector3d& pOrigin, Omen::Vector3d& pNormal) 
{
  
    Omen::Float d = Vector3d::dotProduct(pNormal,pOrigin);
    Omen::Float numer = Vector3d::dotProduct(pNormal,rOrigin) + d;
    Omen::Float denom = Vector3d::dotProduct(pNormal,rVector);
  
  if (denom == 0)  // normal is orthogonal to vector, cant intersect
   return (-1.0f);
  return -(numer / denom);	
}
